#ifndef CIRCLESAXISCALBRATION_H
#define CIRCLESAXISCALBRATION_H

#include <Eigen/Core>
#include <vector>

/**
 * @brief 圆轴标定, 绕一个固定轴旋转, 求这个轴线和两个圆心
 *
 *        最优方法是 calibCirclePlanesNorm2 > calibCirclePlanesNorm
 *
 */
class CirclesAxisCalbration
{
public:
    /// 给定刚体A, B点绕指定轴的点集, 输出 法向量
    static bool calibCirclePlanesNorm(std::vector<Eigen::Vector3d> &pAs, std::vector<Eigen::Vector3d> &pBs,
                                 Eigen::Vector3d &outNorm);
    static bool calibCirclePlanesNorm2(std::vector<Eigen::Vector3d> &pAs, std::vector<Eigen::Vector3d> &pBs,
                                 Eigen::Vector3d &outNorm);
    /// 计算一个平面的法向量
    static bool calibPlanesNorm(std::vector<Eigen::Vector3d> &pAs, Eigen::Vector3d &outNorm);
    /// 给定刚体A, B点绕指定轴的点集, 输出 中心 outCenterA, outCenterB
    static bool calibCirclesAxis(std::vector<Eigen::Vector3d> &pAs, std::vector<Eigen::Vector3d> &pBs,
                                 Eigen::Vector3d &outCenterA, Eigen::Vector3d &outCenterB);
    /// 给定刚体A点集, 输出 中心 outCenterA
    static bool calibCircleCenter(std::vector<Eigen::Vector3d> &pAs, Eigen::Vector3d &outCenterA);

private:
    /// 给定刚体A点集和法向量, 输出 中心 outCenterA, 如果能计算出更加准的法向量 n, 这个就更加的精准
    static bool calibCircleCenter(std::vector<Eigen::Vector3d> &pAs, const Eigen::Vector3d &n,
                                  Eigen::Vector3d &outCenterA);
};

#endif // CIRCLESAXISCALBRATION_H
